#include "Armor.h"

void eraseErrorRepeatArmor(vector<ArmorBox> & armors);
bool armorCompare(const ArmorBox & a_armor, const ArmorBox & b_armor, const ArmorBox & lastArmor);

void ArmorDetector::matchArmors()
{
    for (int i=0; i<lights.size()-1; ++i)
    {
        for (int j=i+1; j<lights.size(); ++j)
        {
            ArmorBox armor = ArmorBox(lights[i], lights[j]);
            if(armor.isSuitableArmor())
            {
                armor.l_index = i;
                armor.r_index = j;
                armors.emplace_back(armor);
            }
        }
        eraseErrorRepeatArmor(armors);
    }
    if (armors.empty()) {
        states = ARMOR_NOT_FOUND; //if armors is empty then set state ARMOR_NOT_FOUND 如果armors目前仍为空，则设置状态为ARMOR_NOT_FOUND
        return; //exit function
    }
    else {
        states = ARMOR_FOUND; //else set state ARMOR_FOUND 如果非空（有装甲板）则设置状态ARMOR_FOUND
        return; //exit function
    }
}

void ArmorDetector::setTargetArmor()
{
    if (states == ARMOR_NOT_FOUND)  targetArmor = ArmorBox(); //not found armr then set a default armor as lastArmor 如果状态为没有找到装甲板，则将lastArmor设置为默认的ArmorBox
    else if (states == ARMOR_FOUND) {
        ArmorBox mva = armors[0]; //mva most valuable armor 最适合打击的装甲板
        for (int i = 1; i < armors.size(); i++) //for circle to select the mva 通过遍历装甲板s获取最佳打击装甲板
        {
            if (armorCompare(armors[i], mva, lastArmor)) mva = armors[i];
        }
        targetArmor = mva; //set the mva as the targetArmor of this frame
    }
    lastArmor = targetArmor; //first set the targetArmor(of last frame) as lastArmor 将上一帧的targetArmor设置为本帧的lastArmor
}

void eraseErrorRepeatArmor(vector<ArmorBox> & armors)
{
    int length = armors.size();
    vector<ArmorBox>::iterator it = armors.begin();
    for (size_t i = 0; i < length; i++)
        for (size_t j = i + 1; j < length; j++)
        {
            if (armors[i].l_index == armors[j].l_index ||
                armors[i].l_index == armors[j].r_index ||
                armors[i].r_index == armors[j].l_index ||
                armors[i].r_index == armors[j].r_index)
            {
                armors[i].getDeviationAngle() > armors[j].getDeviationAngle() ? armors.erase(it + i) : armors.erase(it + j);
            }
        }
}

float getPointsDistance(const Point2f& a, const Point2f& b) {
    float delta_x = a.x - b.x;
    float delta_y = a.y - b.y;
    //return sqrtf(delta_x * delta_x + delta_y * delta_y);
    return sqrt(delta_x * delta_x + delta_y * delta_y);
}

bool armorCompare(const ArmorBox & a_armor, const ArmorBox & b_armor, const ArmorBox & lastArmor)
{
    float a_score = 0;  // shooting value of a_armor a_armor的打击度
    float b_score = 0;  //shooting value of b_armor b_armor的打击度
    a_score += a_armor.armorRect.area(); //area value of a a_armor面积得分
    b_score += b_armor.armorRect.area(); //area value of b b_armor面积得分

    //number(robot type) priorty 设置a、b装甲板的分数
    //setNumScore(a_armor.armorNum, targetNum, a_score);
    //setNumScore(b_armor.armorNum, targetNum, b_score);

    //if (lastArmor.armorNum != 0) {  //if lastArmor.armorRect is not a default armor means there is a true targetArmor in the last frame 上一帧图像中存在目标装甲板
    if(1) {
        float a_distance = getPointsDistance(a_armor.center, lastArmor.center); //distance score to the lastArmor(if exist) 装甲板距离得分，算负分
        float b_distance = getPointsDistance(b_armor.center, lastArmor.center); //distance score to the lastArmor(if exist) 装甲板距离得分，算负分
        a_score -= a_distance * 2;
        b_score -= b_distance * 2;
    }
    return a_score > b_score; //judge whether a is more valuable according their score 根据打击度判断a是否比b更适合打击
}
